'''
2018-1-22
MotionBar script for Asphalt keyboard operation
'''
import serial, win32con, pygame,sys, threading
from win32api import keybd_event

#### datas ####
KEYUP = win32con.KEYEVENTF_KEYUP
KEY_LEFT  = win32con.VK_LEFT
KEY_RIGHT = win32con.VK_RIGHT
KEY_UP    = win32con.VK_UP
KEY_DOWN  = win32con.VK_DOWN
KEY_SPACE = win32con.VK_SPACE

bias = [0.10, 0.00, 0.00, -3.48, -1.64, 0.25]
alpha_lowPass = 0.6

SCR_SIZE = [300,200]
BGC = (0,0,0)    #background color


#### functions ####
kb = keybd_event
delay = pygame.time.delay

def clear():
    ''' to clear the old and irrcorrect data '''
    ser.flushInput(); ser.readline()

def drink_data(string,ifVerify=True):
    ''' 2018-1-22 last modified
        to drink the data out from the string
        data format: "Bax,ay,az,gx,gy,gz,b1,b2,b3E"
    '''
    #verify if the sting is complete
    string = string.strip('\n')
    if ifVerify:
        if string[0] == "B" and string[-1] == "E":
            pass
        else:
            string = "B%s%sE"%("-1,"*8,"-1")
    #now, drink the data
    data = string[1:-1].split(",")
    for i in xrange(6):
        data[i] = float(data[i])
    for i in xrange(3):
        data[i+6] = int(data[i+6])

    return data


def normalize(v):
    model = v[0]**2 + v[1]**2 + v[2]**2
    if model != 0:
        v[0] = v[0]/model; v[1] = v[1]/model; v[2] = v[2]/model
    else:
        v = 0,0,0

    return v


def quit_detect():
    for event in pygame.event.get():
        if event.type == pygame.QUIT:
            ser.close()
            sys.exit()


def visual(v, ifOn, ogn=[150,10]):
    ''' test usage '''
    clor = [255,255,0]
    r = 100
    end = int(ogn[0] + v[2] * r), ogn[1] + int(v[0] * r)
    scr.fill(BGC)
    pygame.draw.line(scr, clor, ogn, end, 5 )
    pygame.draw.circle(scr, clor, end, 5, 0)

    if ifOn: txt = "on %.3f"%v[2]
    else:     txt = "off%.3f"%v[2]
    scr.blit(font.render(txt,0,clor), (5,5))
    pygame.display.flip()


class Controler():
    def __init__(self):
        self.ifRun = False
        self.key = KEY_LEFT    #nothing specil meaning, just an initialization
        #self.lastKey = 0
        self.pwmValue = 0
        self.last_pwmValue = 0
        self.nextT = 0
        self.step = 0
        
    def pwm(self):
        ''' I think a better way is to put the function in another single thread,
            however, I couldn't handle it.
            Since pygame runs fast enough, so I can run it in single thread mode with worry
        '''
        "all units are in millisecond"
        #OPTION = [0, KEYUP]                
        pwm_T = 200
        #pwm_step = 3
        #step_t = pwm_T/pwm_step
        active_t = int(abs(self.pwmValue) * pwm_T); relax_t = pwm_T - active_t
        #active_t = abs(self.pwmValue) * step_t; relax_t = pwm_T - active_t

        if self.last_pwmValue*self.pwmValue < 0:    #when dirction change, release the last key
            kb(self.Key,0,KEYUP,0)
            self.step = 1                           #in order to start a new period later
            self.nextT = 0                          #go to next period imediately
        elif self.last_pwmValue != self.pwmValue:
            self.setp = 1                           #in order to start a new period later, too
            self.nextT = 0                          #go to next period imediately
        
        if self.pwmValue > 0:                                                  #determine which key to press
            self.key = KEY_LEFT
            #kb(self.key, 0, OPTION[self.step], 0)
        elif self.pwmValue < 0:
            self.key = KEY_RIGHT
            #kb(self.key, 0, OPTION[self.step], 0)
        elif self.pwmValue == 0 and self.pwmValue != self.last_pwmValue:       #release only when need
            kb(KEY_LEFT, 0,KEYUP,0)
            kb(KEY_RIGHT,0,KEYUP,0)

        if pygame.time.get_ticks() >= self.nextT:
            if self.step == 0:
                self.step = 1
                self.nextT += relax_t
                if relax_t != 0: kb(self.key, 0, KEYUP, 0)
            elif self.step == 1:
                self.step =0
                self.nextT += active_t
                if active_t != 0: kb(self.key, 0, 0, 0)
        


    def turn(self):
        if self.pwm_value != self.last_pwm_value:
            if self.pwm_value == 0:
                kb(KEY_LEFT,0,KEYUP,0)
                kb(KEY_RIGHT,0,KEYUP,0)
            elif self.pwm_value < 0:
                kb(KEY_RIGHT,0,0,0)
            elif self.pwm_value > 0:
                kb(KEY_LEFT,0,0,0)
            self.last_pwm_value = self.pwm_value


    def drive(self, v, button):
        #print "driving..."
        
        if self.ifRun:
            '''
            v = v[2]
            if v < 0.15 and v > -0.3:self.pwmValue = 0;
            elif v > 0.68:self.pwmValue = 3   #turn left
            elif v > 0.45:self.pwmValue = 2
            elif v > 0.2: self.pwmValue = 1
            elif v < -0.75:self.pwmValue = -3 #turn right
            elif v < -0.53:self.pwmValue = -2
            elif v < -0.2: self.pwmValue = -1  
            '''      
            if abs(v[2]) < 0.13: self.pwmValue = 0
            else:               self.pwmValue = v[2]**3
            self.pwm()
        
        if button[0] == 0:   kb(KEY_SPACE,0,0,0)
        elif button[0] == 1: kb(KEY_SPACE,0,KEYUP,0)
        if button[1] == 0:   kb(KEY_DOWN,0,0,0)
        elif button[1] == 1: kb(KEY_DOWN,0,KEYUP,0)
        if button[2] == 0:
            self.ifRun = not self.ifRun
            print "VKB ", self.ifRun
            delay(500)

    
#### initialization ####
pygame.init()
scr = pygame.display.set_mode(SCR_SIZE)
[ pygame.draw.circle(  scr, [91, 249, 244], (SCR_SIZE[0]/2, SCR_SIZE[1]/2), i*10, 3) for i in range(1, 10) ]
pygame.display.flip() 
font = pygame.font.Font(None, 50)
 
motion = [0,0,0]

driver = Controler()

#t = threading.Thread(target=pwm, name="pwm")
#t.start()
#t.join()

ser = serial.Serial("com3")
clear() 



#### loop ####
bt = pygame.time.get_ticks()
print "all ready!!!"
while True:
    if ser.inWaiting() > 200:
        clear() 
    imu_data = drink_data(ser.readline())
    while imu_data[5] == -1:
        imu_data = drink_data(ser.readline())
    
    button_data = imu_data[6:] # ha ??????????????
    imu_data = [ imu_data[i] - bias[i] for i in xrange(6) ]  #substract the bias
    imu_data[:2] = [ -imu_data[i] for i in xrange(3) ]           #to make the dirction of accelaration same of imu coordinate(the reading is oppsite to the real dirction)
    motion = normalize(motion)
    motion = [ motion[i]*alpha_lowPass + imu_data[i]*(1-alpha_lowPass) for i in xrange(3)]

    #visual(motion, ifOn = driver.ifRun)
    quit_detect()
    driver.drive(motion, button_data)

'''
ser.close()
et = pygame.time.get_ticks()
print (et - bt) /1000.0
print 1000*1000.0/(et - bt) , "Hz"
'''
    
    
